#include "hwt101.hpp"

yj_return hwt101::_read_reg(reg_addr reg, uint16_t *data, uint8_t len)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_reg:i2c is not ready";
        return YJ_ERROR;
    }

    if (!data)
    {
        LOG(ERROR) << "❌ _read_reg:data is null";
        return YJ_ERROR;
    }
    uint8_t *u8_array = reinterpret_cast<uint8_t *>(data);
    uint32_t u8_len = len * sizeof(uint16_t); // 字节长度 = 元素数 × 2

    if (_i2c->read_register(static_cast<uint8_t>(reg), u8_array, u8_len) != YJ_OK)
    {
        LOG(ERROR) << "❌ _read_reg:hwt101 read failed";
        return YJ_ERROR;
    }

    // 交换字节顺序
    // uint16_t temp;
    // for (int i = 0; i < len; i++)
    // {
    //     temp = data[i];
    //     data[i] = (temp >> 8) | (temp << 8);
    // }

    return YJ_OK;
}

yj_return hwt101::_write_reg(reg_addr reg, uint16_t *data, uint8_t len)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_reg:i2c is not ready";
        return YJ_ERROR;
    }

    if (!data)
    {
        LOG(ERROR) << "❌ _read_reg:data is null";
        return YJ_ERROR;
    }
    // 交换字节顺序
    // uint16_t temp;
    // uint16_t u16_temp[len];
    // for (int i = 0; i < len; i++)
    // {
    //     temp = data[i];
    //     u16_temp[i] = (temp >> 8) | (temp << 8);
    // }

    uint8_t *u8_array = reinterpret_cast<uint8_t *>(data);
    uint32_t u8_len = len * sizeof(uint16_t); // 字节长度 = 元素数 × 2

    if (_i2c->write_register(static_cast<uint8_t>(reg), u8_array, u8_len) != YJ_OK)
    {
        LOG(ERROR) << "❌ _write_reg:hwt101 write failed";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return hwt101::read_single_reg(reg_addr reg, uint16_t *reg_out)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_single_reg:device i2c is not open";
        return YJ_ERROR;
    }
    if (!reg_out)
    {
        LOG(ERROR) << "❌ read_single_reg:hwt101 reg_out is null";
        return YJ_ERROR;
    }

    if (_read_reg(reg, reg_out, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_single_reg:hwt101 read reg failed";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return hwt101::write_single_reg(reg_addr reg, uint16_t reg_out)
{
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ write_single_reg:device i2c is not open";
        return YJ_ERROR;
    }

    if (_write_reg(reg, &reg_out, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ write_single_reg:hwt101 read reg failed";
        return YJ_ERROR;
    }
    return YJ_OK;
}

hwt101::hwt101(std::string dev_name,
               i2c_v1 *i2c,
               uint8_t device_address) : _dev_name(dev_name),
                                         _i2c(i2c),
                                         _device_address(device_address),
                                         _last_yaw(0),
                                         _yaw_offset(0)
{
    _flag = YJ_ERROR;
}
hwt101::~hwt101()
{
    if (_i2c)
    {
        _i2c->close();
    }
}

yj_return hwt101::read_status()
{
    return _flag;
}
yj_return hwt101::init()
{
    double current_lsb, cal;
    uint16_t reg_buff;

    if (_i2c->open() != YJ_OK)
    {
        goto end;
    }

    if (_i2c->set_address(_device_address) != YJ_OK)
    {
        goto end;
    }

    if (reset() != YJ_OK)
    {
        goto end;
    }

    LOG(INFO) << "✔️ hwt101 init ok";
    print_config();
    _flag = YJ_OK;
    return YJ_OK;
end:
    _flag = YJ_ERROR;
    return YJ_ERROR;
}

yj_return hwt101::reset()
{
    uint16_t reg_temp;
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ reset: i2c is not open";
        return YJ_ERROR;
    }

    // 重启
    reg_temp = 0x00FF;
    if (_write_reg(save_reg_addr, &reg_temp, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ reset:hwt101 reset to factory error";
        return YJ_ERROR;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // 设置速率为200HZ
    reg_temp = 0x0B;
    if (_write_reg(rrate_reg_addr, &reg_temp, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ reset:hwt101 set rate speed error";
        return YJ_ERROR;
    }

    // Z轴角度归0
    reg_temp = 0x00;
    if (_write_reg(cali_yaw_reg_addr, &reg_temp, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ reset:hwt101 reset Z angle to zero error";
        return YJ_ERROR;
    }

    // 关闭44   开启自动校准
    reg_temp = 0x00;
    if (_write_reg(no_auto_cali_reg_addr, &reg_temp, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ reset:hwt101 close auto calibration error";
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return hwt101::read_yaw(double *yaw)
{
    uint16_t yaw_reg;
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_yaw: i2c is not open";
        return YJ_ERROR;
    }

    if (!yaw)
    {
        LOG(ERROR) << "❌ read_yaw: pra yaw is null";
        return YJ_ERROR;
    }

    if (_read_reg(yaw_reg_addr, &yaw_reg, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_yaw: read yaw reg error";
        return YJ_ERROR;
    }
    // LOG(INFO) << "yaw_reg: " << yaw_reg;
    *yaw = (double)(static_cast<int16_t>(yaw_reg)) * 180 / 32768;
    return YJ_OK;
}

yj_return hwt101::read_yaw_for_pid(double *yaw)
{
    uint16_t yaw_reg;
    double step_angle,yaw_temp;
    if (_i2c->is_open() != YJ_OK)
    {
        LOG(ERROR) << "❌ read_yaw_for_pid: i2c is not open";
        return YJ_ERROR;
    }

    if (!yaw)
    {
        LOG(ERROR) << "❌ read_yaw_for_pid: pra yaw is null";
        return YJ_ERROR;
    }

    if (_read_reg(yaw_reg_addr, &yaw_reg, 1) != YJ_OK)
    {
        LOG(ERROR) << "❌ read_yaw_for_pid: read yaw reg error";
        return YJ_ERROR;
    }
    // LOG(INFO) << "yaw_reg: " << yaw_reg;
    yaw_temp= (double)(static_cast<int16_t>(yaw_reg)) * 180 / 32768;

    step_angle = yaw_temp - _last_yaw ;
    if(step_angle < -180) 
    {
        step_angle += 360;
    }
    else if(step_angle > 180)
    {
        step_angle -= 360;
    }
    _last_yaw = yaw_temp;
    
    _yaw_offset += step_angle;
    // LOG(INFO) << "step_angle: " << step_angle << " yaw_temp: " << yaw_temp << " _last_yaw: " << _last_yaw << " _yaw_offset: " << _yaw_offset;
    *yaw = _yaw_offset;
    
    return YJ_OK;
}

void hwt101::print_config()
{
    LOG(INFO) << "=======================hwt101 config=======================";
    LOG(INFO) << "dev name: " << _dev_name;
    LOG(INFO) << "i2c bus name: " << _i2c->get_dev_path();
    LOG(INFO) << "i2c addr: 0x" << std::hex << std::setfill('0') << std::setw(2) << std::uppercase << static_cast<uint16_t>(_device_address);
    LOG(INFO) << "=======================hwt101 config=======================";
}